#include "camselect.h"

Camselect::Camselect(){};
Camselect::~Camselect(){};

void Camselect::discheck(int i,vector<Eigen::VectorXf> model_circle,Mat img_disp,Point left_up,Point right_down,float &dis_bucket)
{
    int depth_sum = 0;
    int depth_num = 0;
    for(int m=left_up.y;m<right_down.y-20;m++)
      for(int j=left_up.x+15;j<right_down.x-15;j++)
      {
        if(img_disp.at<unsigned short>(m,j)>200&&img_disp.at<unsigned short>(m,j)<10000)
        {
          depth_sum+=img_disp.at<unsigned short>(m,j);
          depth_num++;
        }
      }
    dis_bucket = fabs((float)depth_sum/depth_num-fabs(600-model_circle[i][1])*10);
    //根据距离进行匹配
    cout<<"相机距离:: "<<(float)depth_sum/depth_num<<endl;
    cout<<"距离差:: "<<dis_bucket<<endl;
}

void Camselect::varcheck(Mat img_left,Point center,Point left_up,Point right_down,float &var)
{
    vector<float> Point_Z;
    float avg=0;
        for(int row=center.y;row<right_down.y-20;row++)
        for(int col=left_up.x+15;col<right_down.x-15;col++)
        {
          int Z=img_left.at<uchar>(row,col);
          Point_Z.push_back(Z);
          avg=avg+Z;
        }
        avg/=Point_Z.size();
        for(int row=center.y;row<right_down.y-20;row++)
        for(int col=left_up.x+15;col<right_down.x-150;col++)
        {
          float Z=img_left.at<uchar>(row,col);
          var+=abs(Z-avg);
        }
      var/=Point_Z.size();
      cout<<"方差:: "<<var<<endl;
}

void Camselect::areacheck(Mat img_disp,Point center,Point left_up,Point right_down,float &dis_area)
{
    int depth_sum_down = 0;
    float depth_num_down = 0.1;
    for(int row=left_up.y;row<center.y;row++)
      for(int col=left_up.x+15;col<right_down.x-15;col++)
      {
        if(img_disp.at<unsigned short>(row,col)>200&&img_disp.at<unsigned short>(row,col)<10000)
        {
          depth_sum_down+=img_disp.at<unsigned short>(row,col);
          depth_num_down++;
        }
      }

    int depth_sum_up = 0;
    float depth_num_up = 0.1;
    for(int row=2*left_up.y-center.y;row<left_up.y;row++)
      for(int col=left_up.x+15;col<right_down.x-15;col++)
      {
        if(img_disp.at<unsigned short>(row,col)>200&&img_disp.at<unsigned short>(row,col)<10000)
        {
          depth_sum_up+=img_disp.at<unsigned short>(row,col);
          depth_num_up++;
        }
      }
    dis_area = fabs((float)depth_sum_down/depth_num_down-(float)depth_sum_up/depth_num_up);
    cout<<"桶及其上方区域深度差:  "<<dis_area<<endl;
}

void Camselect::camcheck(float camera_height,vector<double> cam_information,Mat img_left,Mat img_disp,vector<Eigen::VectorXf> &model_circle)
{
    for(int i=0;i<model_circle.size();)
    {
       Point center;
       Point left_up;
       Point right_down;
       center.x=cam_information[0]*((float)(model_circle[i][0]-376)/(600-model_circle[i][1]))+cam_information[2];
       left_up.y=(cam_information[5]-cam_information[0]*(1.2-camera_height)/(600-model_circle[i][1])*100)>0?(cam_information[5]-cam_information[0]*(float)camera_height/(600-model_circle[i][1])*100):0;
       right_down.y=(cam_information[5]+(cam_information[0]*camera_height/(600-model_circle[i][1])*100))>480?480:(cam_information[5]+(cam_information[0]*(float)camera_height/(600-model_circle[i][1])*100));
       center.y=(left_up.y+right_down.y)/2;
       left_up.x=center.x-(cam_information[0]*0.25/(600-model_circle[i][1])*100);
       right_down.x=center.x+(cam_information[0]*0.25/(600-model_circle[i][1])*100);


       rectangle(img_left,left_up,right_down, 0,3);

       float dis_bucket=0.0;
       discheck(i, model_circle,img_disp,left_up,right_down,dis_bucket);

        if(dis_bucket>10000)
        {
          model_circle.erase(model_circle.begin()+i);
        }

        else{
          float var=0.0;
          varcheck(img_left,center,left_up,right_down,var);
          if(var>th_graypic)
          {model_circle.erase(model_circle.begin()+i);}
          else{
        if((2*left_up.y-center.y)>0)
        {
          float dis_area=0.0;
          areacheck(img_disp,center,left_up,right_down,dis_area);
        if(dis_area<0)
        {model_circle.erase(model_circle.begin()+i);}
        else{++i;}
        }
        else{++i;}
        }
        }
      }
}
